#include <stdio.h>
#include <string.h>
#include <math.h>
#include "userstruct.h"

#pragma region 本算法函数声明_18root
int    Read18EPHFromFile(FILE *hReadFile);
void LEOCTSPosVelFromEph(double Secs, double NavParaKepler[], CGS2000_PAR *mCGS2000Para,double outpos[]);
void   clac18root();//计算ECEF数据生成文件
#pragma endregion

#pragma region 外部公共函数声明
int    GetReadCGS2000Para(CGS2000_PAR  *mCGS2000Para);
double Frac(double x);//Fractional part of a number (y=x-[x])
double Modulo(double x, double y);//x mod y
double EccAnom (double M, double e);
double delt_WeekSecT(WeekSecTimeA *_t1,WeekSecTimeA *_t2);//计算两个WeekSecTimeA之间的秒数差
void   WeekSecTimeA_validity(WeekSecTimeA *_tm);/*将时间表达形式规范化*/
#pragma endregion


//<-----------全局变量-----------------
X32          mBrdEPH;          //星历存储变量， 注意：22参数仅用18参数
CGS2000_PAR  mCGS2000Para;     //CGS2000坐标系参数定义
double       FitArcLength=3600;//总拟合弧长3600秒--60分钟
//-----------全局变量----------------->

int main(int argc, char *argv[])
{
	printf("hello c world.\n");

	//1-----读测试18根数文件root18ANSI.txt到全局变量中--mBrdEPH
	memset(&mBrdEPH, 0, sizeof(mBrdEPH)); //初始化星历18参数初值全部为0
	FILE *hEphFile;
	hEphFile=fopen("./root18ANSI.txt","r");
    if(Read18EPHFromFile(hEphFile))
	{
		printf("open input file error and exit!\n");
		return -1;
	}
    if(hEphFile)fclose(hEphFile);
    
	//2-----下面计算18根数生成位置文件
    clac18root();
    printf("calc finished.\n");

    return 0;
}

/*
    使用全局变量 mBrdEPH 保存读取的星历文件
    星历文件由提供程序生成
*/
int  Read18EPHFromFile(FILE *hReadFile)
{
    char    ReadChars[300];
    char    TempChars[100];
    char    ReadStr[300];
    int     NoGroup=0;
    int     TempInt=0;
    if (hReadFile == NULL)return 1;
    fseek(hReadFile, SEEK_SET, 0 );//置于文件的开头
    //例子文件18根数一共120行，含6个数据
    while(!feof(hReadFile) )
    {
        fgets(ReadChars, 300,  hReadFile );
        strcpy(ReadStr,ReadChars);
        //printf("%s\n",ReadChars);  //ReadChars是ANSI编码
		char *subStr1 = "FitArc";
		char *subStr2 = "**";
		char *ch1 = strstr(ReadStr, subStr1);
		char *ch2 = strstr(ReadStr, subStr2);
        if (ch1 != NULL)
        {
            printf("find the id\n");
            sscanf(ReadStr," %s %lf ",TempChars, &FitArcLength);
            FitArcLength *= 60.0;//min to second
        }
        else if (ch2 != NULL)
        {
            printf("ReadStr:%s\n",ReadStr);//** i 869  360004.000000
            sscanf(ReadStr," %s %d %d %lf", TempChars, &TempInt, //TempChars, &TempInt用于格式化输入参数
                    &mBrdEPH.RNSSbrd_arry[NoGroup].ref_epoch_week,  //869 周
                    &mBrdEPH.RNSSbrd_arry[NoGroup].ref_epoch_sow ); //360004.000000星历参考历元，秒，实际上这个数也是toe
            //下面是每组18根数的数据
            fgets(ReadChars, 300,  hReadFile );
            strcpy(ReadStr,ReadChars);
            sscanf(ReadStr," %s %lf ", TempChars, &mBrdEPH.RNSSbrd_arry[NoGroup].a);

            fgets(ReadChars, 300,  hReadFile );
            strcpy(ReadStr,ReadChars);
            sscanf(ReadStr," %s %lf ", TempChars, &mBrdEPH.RNSSbrd_arry[NoGroup].e);

            fgets(ReadChars, 300,  hReadFile );
            strcpy(ReadStr,ReadChars);
            sscanf(ReadStr," %s %lf ", TempChars, &mBrdEPH.RNSSbrd_arry[NoGroup].i0);

            fgets( ReadChars, 300,  hReadFile );
            strcpy(ReadStr,ReadChars);
            sscanf(ReadStr," %s %lf ", TempChars, &mBrdEPH.RNSSbrd_arry[NoGroup].OMEGA0);

            fgets(ReadChars, 300,  hReadFile );
            strcpy(ReadStr,ReadChars);
            sscanf(ReadStr," %s %lf ", TempChars, &mBrdEPH.RNSSbrd_arry[NoGroup].omega);

            fgets( ReadChars, 300,  hReadFile );
            strcpy(ReadStr,ReadChars);
            sscanf(ReadStr," %s %lf ", TempChars, &mBrdEPH.RNSSbrd_arry[NoGroup].M0);

            fgets(ReadChars, 300,  hReadFile );
            strcpy(ReadStr,ReadChars);
            sscanf(ReadStr," %s %lf ", TempChars, &mBrdEPH.RNSSbrd_arry[NoGroup].deltan);

            fgets( ReadChars, 300,  hReadFile );
            strcpy(ReadStr,ReadChars);
            sscanf(ReadStr," %s %lf ", TempChars, &mBrdEPH.RNSSbrd_arry[NoGroup].OMEGAdot);

            fgets(ReadChars, 300,  hReadFile );
            strcpy(ReadStr,ReadChars);
            sscanf(ReadStr," %s %lf ", TempChars, &mBrdEPH.RNSSbrd_arry[NoGroup].idot);

            fgets(ReadChars, 300,  hReadFile );
            strcpy(ReadStr,ReadChars);
            sscanf(ReadStr," %s %lf ", TempChars, &mBrdEPH.RNSSbrd_arry[NoGroup].cus);

            fgets(ReadChars, 300,  hReadFile );
            strcpy(ReadStr,ReadChars);
            sscanf(ReadStr," %s %lf ", TempChars, &mBrdEPH.RNSSbrd_arry[NoGroup].cuc);

            fgets(ReadChars, 300,  hReadFile );
            strcpy(ReadStr,ReadChars);
            sscanf(ReadStr," %s %lf ", TempChars, &mBrdEPH.RNSSbrd_arry[NoGroup].cis );

            fgets(ReadChars, 300,  hReadFile );
            strcpy(ReadStr,ReadChars);
            sscanf(ReadStr," %s %lf ", TempChars, &mBrdEPH.RNSSbrd_arry[NoGroup].cic );

            fgets(ReadChars, 300,  hReadFile );
            strcpy(ReadStr,ReadChars);
            sscanf(ReadStr," %s %lf ", TempChars, &mBrdEPH.RNSSbrd_arry[NoGroup].crs );

            fgets(ReadChars, 300,  hReadFile );
            strcpy(ReadStr,ReadChars);
            sscanf(ReadStr," %s %lf ", TempChars, &mBrdEPH.RNSSbrd_arry[NoGroup].crc );

            fgets(ReadChars, 300,  hReadFile );
            strcpy(ReadStr,ReadChars);
            sscanf(ReadStr," %s %lf ", TempChars, &mBrdEPH.RNSSbrd_arry[NoGroup].adot );

            fgets(ReadChars, 300,  hReadFile );
            strcpy(ReadStr,ReadChars);
            sscanf(ReadStr," %s %lf ", TempChars, &mBrdEPH.RNSSbrd_arry[NoGroup].dot_deltan );

            fgets(ReadChars, 300,  hReadFile );
            strcpy(ReadStr,ReadChars);
            sscanf(ReadStr," %s %lf ", TempChars, &mBrdEPH.RNSSbrd_arry[NoGroup].ref_epoch_sow );
            NoGroup++;
        }
    }

    mBrdEPH.Ngp = NoGroup;//星历组数 <=24
    printf("mBrdEPH.Ngp=%d\n",mBrdEPH.Ngp);//sat18root.rst文件一共有6组18根数数据
    printf("FitArcLength=%f\n",FitArcLength);
    printf("read ok\n");
    return 0;
}

//计算ECEF数据生成文件
void clac18root()
{
    /*时间变量，用于标识需要计算卫星状态信息*/
    WeekSecTimeA       GoalTime;
    char               OutStrChars[200];
    /*用于保存CGS2000的有关参数的定义常量*/
    double  GoalSatPos[6];      //保存目标时刻卫星的位置
    double  CurEphPara[18];     //使用的卫星广播星历，矢量表达
    int  i ,m,  nn; //临时变量
    FILE *hRstFile;
    /*时间变量：周＋秒，保存当前星历的参考时间*/
    WeekSecTimeA  CurEphRefTime;

    GetReadCGS2000Para(&mCGS2000Para);//初始化CGS2000坐标系定义的有关参数
    if ( (hRstFile = fopen("ECEF_EPH18Para_POSVEL.rst","wt") ) == NULL)
        return ;

    //临时设置的计算卫星位置和速度时间的起点
    GoalTime.Week = mBrdEPH.RNSSbrd_arry[0].ref_epoch_week;
    GoalTime.Secs = mBrdEPH.RNSSbrd_arry[0].ref_epoch_sow;

    //如果没有有效的星历，
    if (mBrdEPH.Ngp <= 0)return ;

    {
        //计算180个点
        for(  nn=0; nn< 180; nn++ )
        {
            /*第一步，开始搜寻对应的有效广播星历的位置*/
            for(int m=0; m<mBrdEPH.Ngp; m++)
            {
                /*当前星历参考时间的周、秒*/
                CurEphRefTime.Week =  mBrdEPH.RNSSbrd_arry[m].ref_epoch_week;
                CurEphRefTime.Secs =  mBrdEPH.RNSSbrd_arry[m].ref_epoch_sow;

                if (delt_WeekSecT(&GoalTime,&CurEphRefTime) < 0)//目标时间较早
                {
                    /*目标时间与星历的参考时间不对应，目标时间太早*/
                    if (m == mBrdEPH.Ngp - 1)return;
                    continue;
                }
                else //如果星历参考的时间较早
                {
                    /*暂时认为，星历有效期为2个小时，且有效期参考的起点时间就是星历数据
                         的参考时间，这与目前系统的设计是一致的！
                         这个地方的设置有点诀窍，1小时与实际使用情况相同；最大可设4小时，与
                         可用时间跨度相同。
                       */
                    if (delt_WeekSecT(&GoalTime,&CurEphRefTime) > FitArcLength) //星历有效期过了
                    {
                        /*目标时间与星历的参考时间不对应，目标时间太晚*/
                        if (m == mBrdEPH.Ngp - 1)return ;
                        continue;
                    }
                }

                i = 0;
                CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[m].a;
                //qDebug()<<"CurEphPara[0]:"<<QString::number(CurEphPara[0],'f',6);
                CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[m].e;
                CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[m].i0;

                CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[m].OMEGA0;
                CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[m].omega;
                CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[m].M0;

                CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[m].deltan;
                CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[m].OMEGAdot;
                CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[m].idot;

                CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[m].cus;
                CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[m].cuc;
                CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[m].cis;

                CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[m].cic;
                CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[m].crs;
                CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[m].crc;

                CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[m].adot;
                CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[m].dot_deltan;

                //Toe
                CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[m].ref_epoch_sow;
                /*
                for( i=0; i<=17; i++ )
                {
                    sprintf( OutStrChars, "\n %s %15.12E ", EphParaName[i], CurEphPara(i) );
                    Tempstr = OutStrChars;
                    DisplayMemo->Lines->Add( Tempstr );
                }
                */

                LEOCTSPosVelFromEph(GoalTime.Secs,CurEphPara,&mCGS2000Para,GoalSatPos);//根据18根数计算卫星ECEF一组数据

                sprintf( OutStrChars,"%09d   %03d   %07.2lf   %12.3lf   %12.3lf   %12.3lf   %09.4lf   %09.4lf   %09.4lf",
                         nn*60,GoalTime.Week, GoalTime.Secs, GoalSatPos[0], GoalSatPos[1],
                         GoalSatPos[2], GoalSatPos[3], GoalSatPos[4], GoalSatPos[5]);
                //Tempstr = OutStrChars;
                //DisplayMemo->Lines->Add( Tempstr );
                fprintf(hRstFile, " %s \n", OutStrChars);
                break;
            }
            GoalTime.Secs += 60;  //一分钟一个结果
            WeekSecTimeA_validity(&GoalTime);//GoalTime.validity();
        }
    }
    
    if( hRstFile)fclose( hRstFile );
}

/*
    函数目的：根据广播星历计算给定特定时间的卫星位置：
    输入：   秒Secs (当前周的妙计数，BD2T)
             对应的广播星历： EphPara：依次:
      0  double a;                      // a
      1  double e;                      // 偏心率e
      2  double omega;                  // 近地点幅角
      3  double deltan;                 // 卫星平均运动速率与计算之差
      4  double M0;                     // 参考时间的平近点角
      5  double OMEGA0;                 // 周开始时刻计算的升交点经度，clc modified 2006-09-06
      6  double OMEGAdot;               // 升交点经度变化率
      7  double i0;   	                // 参考时间的轨道倾角
      8  double idot; 	                // 轨道倾角变化率
      9  double cuc;  	                // 纬度幅角的余弦调和改正项的振幅
      10  double cus;  	                // 纬度幅角的正弦调和改正项的振幅
      11  double crc;  	                // 轨道半径的余弦调和改正项的振幅
      12  double crs;  	                // 轨道半径的正弦调和改正项的振幅
      13  double cic;  	                // 轨道倾角的余弦调和改正项的振幅
      14  double cis;                   // 轨道倾角的余弦调和改正项的振幅
      15  double dotA;
      16  double deltan0
      17  double toe;                   // 星历参考历元，秒
    返回：Vector(3)，卫星在CGS2000中的位置。
*/
void LEOCTSPosVelFromEph(double Secs, double NavParaKepler[], CGS2000_PAR *mCGS2000Para,double outpos[])
{
    // Variables
    double  A0, Ak, e, n0, nA, dotnA, M0, Mk,  Ek;
    double  Omega, Parai,  Omegak,tk;
    double  Vk,fik,delta_u, delta_r,delta_i;
    double  uk,rk,ik,xk,yk,tempf;
    double  sinE,cosE,sqrt1_e2;
    double  r[6];

    //为了解算卫星速度专用的变量
    double  ERate,fikRate,ukRate,rkRate,ikRate,xkRate,ykRate,OmegakRate;
    //利用GPS星历参数解算算法计算对应时刻卫星位置
    tk = Secs - NavParaKepler[17];
    if ( tk < -302400 ) tk += 604800;

    Ak  = NavParaKepler[0] + NavParaKepler[15]*tk;
    A0 =  NavParaKepler[0];
    Omega = NavParaKepler[3];

    //n0 = sqrt (mCGS2000Para.GM/(Ak*Ak*Ak));
    n0 = sqrt (mCGS2000Para->GM/(A0*A0*A0));  //20230229 YJH
    nA = n0 + NavParaKepler[6] + 0.5*NavParaKepler[16]*tk;

    M0 = NavParaKepler[5];
    Mk = M0 +nA*tk;

    e  = NavParaKepler[1];
    // Eccentric anomaly
    Ek  = EccAnom(Mk,e);

    Parai = NavParaKepler[2];

    Vk = 2.0*atan( sqrt((1+e)/(1-e))*tan(Ek/2.0) );

    fik = Vk + NavParaKepler[4];

    ERate = (nA+0.5*NavParaKepler[16]*tk)/(1.0-e*cos(Ek)); //20230229 YJH
    //ERate =  nA/(1.0-e*cos(Ek));

    //fikRate = sqrt((1+e)/(1-e))*cos(Vk/2.0)*cos(Vk/2.0)*ERate/(cos(E/2.0)*cos(E/2.0));
    fikRate = sqrt( 1.0-e*e ) * ERate /( 1.0-e*cos(Ek) );

    delta_u = NavParaKepler[10]*cos( 2.0*fik)
            + NavParaKepler[9]*sin( 2.0*fik);
    delta_r = NavParaKepler[14]*cos( 2.0*fik)
            + NavParaKepler[13]*sin( 2.0*fik);
    delta_i = NavParaKepler[12]*cos( 2.0*fik)
            + NavParaKepler[11]*sin( 2.0*fik);

    uk = fik+delta_u;
    rk = Ak*(1.0-e*cos(Ek))+ delta_r;
    ik = Parai+delta_i+NavParaKepler[8]*tk;

    /*以上三者对应的变化率*/
    ukRate = ( 1.0 + 2.0*NavParaKepler[9]*cos(2.0*fik) - 2.0*NavParaKepler[10]*sin(2.0*fik) ) * fikRate;
    rkRate  =  Ak*e*sin(Ek)*ERate;
    rkRate +=  2.0* NavParaKepler[13]*cos(2.0*fik) * fikRate;
    rkRate -=  2.0* NavParaKepler[14]*sin(2.0*fik) * fikRate;
    rkRate +=  NavParaKepler[15] * (1.0 - e*cos(Ek) );
    ikRate = NavParaKepler[8] +
            2.0*( NavParaKepler[11]*cos(2.0*fik) - NavParaKepler[12]*sin(2.0*fik) ) * fikRate;

    xk = rk*cos(uk);
    yk = rk*sin(uk);

    xkRate = rkRate * cos(uk) - rk * sin(uk) * ukRate;
    ykRate = rkRate * sin(uk) + rk * cos(uk) * ukRate;

    //地固系中的坐标、速度求解
    Omegak = Omega + ( NavParaKepler[7]-mCGS2000Para->omega_earth )*tk - mCGS2000Para->omega_earth*NavParaKepler[17];

    OmegakRate = NavParaKepler[7] - mCGS2000Para->omega_earth;

    //计算卫星在地心固定坐标系中的直角坐标
    r[0] = xk*cos( Omegak ) - yk *cos( ik ) * sin( Omegak );
    r[1] = xk*sin( Omegak ) + yk *cos( ik ) * cos( Omegak );
    r[2] = yk*sin( ik );

    /*12.当前时刻卫星在J2000中的三维速度*/
    r[3] = ((((xkRate * cos(Omegak))
              - ((ykRate * sin(Omegak)) * cos(ik)))
             + (((yk * sin(Omegak)) * sin(ik)) * ikRate))
            - ((xk * sin(Omegak)) * OmegakRate))
            - (((yk * cos(Omegak)) * cos(ik)) * OmegakRate);

    r[4] = ((((xkRate * sin(Omegak))
              + ((ykRate * cos(Omegak)) * cos(ik)))
             - (((yk * cos(Omegak)) * sin(ik))* ikRate))
            + ((xk * cos(Omegak)) * OmegakRate))
            - (((yk * sin(Omegak)) * cos(ik)) * OmegakRate);

    r[5] = ykRate * sin(ik) + yk*cos(ik)*ikRate;
    //return r;

    outpos[0]=r[0];
    outpos[1]=r[1];
    outpos[2]=r[2];
    outpos[3]=r[3];
    outpos[4]=r[4];
    outpos[5]=r[5];
}

